/*
 * Capstone.c
 *
 * Created: 1/11/2013 5:30:11 PM
 *  Author: Alexander
 */

#include "Capstone.h"
AxesVector_t Angle;
AxesVector_t Current_Vector;
AxesVector_t Previous_Vector;

/**************************************************************************
* Method:		main
* FullName:		main
* Access:		public 
* Returns:		int
* Qualifier:	
* Parameter:	void
**************************************************************************/
int main(void)
{	
	AvrXSetKernelStack(0);
		
	AvrXSetSemaphore(&EEPromMutex);
	AVRX_HARDWARE_SETUP_COMMANDS;
	
	stderr = stdout = stdin = &uart_stream; /* Redirecting the standard I/O */

	UARTInit(); /* Initial UART */
	AvrXRunTask(TCB(Gyro));
	AvrXRunTask(TCB(GyroAverage));
	
	Epilog();
	while(1);
}

/**************************************************************************
* Method:		GyroAverage
* FullName:		GyroAverage
* Access:		public 
* Returns:		void
* Qualifier:	
* Parameter:	void
**************************************************************************/
void GyroAverage(void)
{	
	int16_t angle_x, angle_y, angle_z, count;
	TimerControlBlock calculateAngle;
	while(1)
	{	
		count = Get_Count(&Angle);
		
//		Scalar_Divide(&Angle, count);
		Scalar_Divide(&Angle, 2);
		//Scalar_Multiply(&Angle, 2);
		Add_Vectors(&Previous_Vector, &Angle);
		
		BeginCritical();
		printf("X: %6d \nY: %6d \nZ: %6d\n\n", Previous_Vector.AXIS_X, Previous_Vector.AXIS_Y, Previous_Vector.AXIS_Z);
		EndCritical();
		Vector_Init(&Angle);
		AvrXSetSemaphore(&turnServos);
		AvrXDelay(&calculateAngle, 500);
	}
}

/**************************************************************************
* Method:		Gyro
* FullName:		Gyro
* Access:		public 
* Returns:		void
* Qualifier:	
* Parameter:	void
**************************************************************************/
void Gyro(void)
{	
	//	InitSPI();	 
	Gyro_Init();

	Vector_Init(&Angle);
	Vector_Init(&Previous_Vector);
	Vector_Init(&Current_Vector);
	AxesVector_t zeroRate = Zero_Rate_Calc();
	TimerControlBlock gyroCalc;
	 
	while(1)
	{
		Calculate_Angle(&zeroRate, &Current_Vector, &Angle);
		AvrXDelay(&gyroCalc, GyroDelay);
	}
}
 
 //************************************
 // Method:    Pump
 // FullName:  Pump
 // Access:    public 
 // Returns:   void
 // Qualifier:
 // Parameter: void
 //************************************
 void Pump(void)
 {	 
	MessageControlBlock PumpBackwardDone, PumpForwardDone;
	TimerControlBlock PumpDuration; 
	Motor_Driver_Init();
	Mutex PumpForward;
 }
 
 void Pump_Forward()
 {
	while(1)
	{
		Motor_Forward();
		Motor_Off();
	}		 
 }
 
 void Pump_Backward()
 {

	 while(1)
	 {
		 Motor_Backward();
		 Motor_Off();	 
	 }		
	 
 }
// 
// CTASKFUNC(Pressure_Sensor)
// {
// 	
// }
// 
// CTASKFUNC(Pump)
// {
// 	
// }
// 
// CTASKFUNC(Servos)
// {
// 	
// }

AVRX_SIGINT(AVRX_HARDWARE_TIMER_SIGNAL)
{
	IntProlog();
	AvrXTimerHandler();
	Epilog();
}
